//
// Created by 86156 on 25-7-5.
//

#include "pid_driver.h"
#include <math.h>

#include "control_driver.h"
#include "math_tool_driver.h"

//
// void PID_Init(PID_t *pid, float Kp, float Ki, float Kd, float MaxI, float MaxOut)
// {
//     pid->Kp = Kp;
//     pid->Ki = Ki;
//     pid->Kd= Kd;
//     pid->Iout_Max = MaxI;
//     pid->Output_Max = MaxOut;
// }

void PID_Position_Cal(PID_t *pid,float target,float real)
{
    pid->Err = target - real;
    pid->Differ=pid->Err - pid->Err_last;

    pid->POut=pid->Kp * pid->Err;
    pid->IOut=pid->Ilimit_flag * pid->Ki * pid->Integral;
    pid->DOut=pid->Kd * pid->Differ;


    if(fabsf(pid->Err)>pid->Ilimit)   //积分分离
    {pid->Ilimit_flag = 0;}

    else {
        pid->Ilimit_flag = 1;                               //加入积分(只有测量值在-PID->Ilimit~PID->Ilimit 范围内时才加入积分)
        pid->Integral += pid->Err;                        //对误差进行积分
        LimitMax(pid->Integral,pid->Irang);
    }
    pid->totalOut = pid->POut + pid->IOut + pid->DOut;
    if (pid->totalOut > pid->Output_Max) pid->totalOut = pid->Output_Max;
    else if (pid->totalOut < -pid->Output_Max) pid->totalOut = -pid->Output_Max;
    pid->Err_last = pid->Err;
}

void PidParameter_Init(PID_t *PID_ROL_Angle,PID_t *PID_YAW_Angle,PID_t *PID_Depth,PID_t *PID_PIT_Angle,PID_t *PID_View, PID_t *PID_ROL_Speed,PID_t *PID_PIT_Speed, PID_t *PID_YAW_Speed)
{

    PID_ROL_Angle->Kp =-2.0f; //6
    PID_ROL_Angle->Ki =0;
    PID_ROL_Angle->Kd =0; //2
    PID_ROL_Angle->Ilimit_flag = 0;//禁用积分分离
    PID_ROL_Angle->Irang = 120; //积分限幅
    PID_ROL_Angle->Output_Max = 70;

    PID_ROL_Speed->Kp =0.5f;
    PID_ROL_Speed->Ki =0;
    PID_ROL_Speed->Kd =0;
    PID_ROL_Speed->Ilimit_flag = 0;//禁用积分分离
    PID_ROL_Speed->Irang = 120; //积分限幅
    PID_ROL_Speed->Output_Max = 50;

    PID_PIT_Angle->Kp =2.0f; //
    PID_PIT_Angle->Ki =0;
    PID_PIT_Angle->Kd =0;
    PID_PIT_Angle->Ilimit_flag = 0;//禁用积分分离
    PID_PIT_Angle->Irang = 120; //积分限幅
    PID_PIT_Angle->Output_Max = 80;

    PID_PIT_Speed->Kp =-2.0f;
    PID_PIT_Speed->Ki =0;
    PID_PIT_Speed->Kd =0;
    PID_PIT_Speed->Ilimit_flag = 0;//禁用积分分离
    PID_PIT_Speed->Irang = 120; //积分限幅
    PID_PIT_Speed->Output_Max = 100;

    PID_YAW_Angle->Kp =2.0f; //5
    PID_YAW_Angle->Ki =0;
    PID_YAW_Angle->Kd =0; //0.5
    PID_YAW_Angle->Ilimit = 10;
    PID_YAW_Angle->Irang = 20; //积分限幅
    PID_YAW_Angle->Output_Max = 100;

    PID_YAW_Speed->Kp =-0.5f;
    PID_YAW_Speed->Ki =0;
    PID_YAW_Speed->Kd =0;
    PID_YAW_Speed->Ilimit_flag = 120;
    PID_YAW_Speed->Irang = 20;
    PID_YAW_Speed->Output_Max = 50;


    PID_Depth->Kp =-0.5f; //5
    PID_Depth->Ki =0; //0.00001
    PID_Depth->Kd =0;
    PID_Depth->Ilimit_flag = 1;//启用积分分离
    PID_Depth->Ilimit = 10;
    PID_Depth->Irang = 80; //积分限幅
    PID_Depth->Output_Max = 100;

    PID_View->Kp=0; //0
    PID_View->Ki=0;
    PID_View->Kd=0; //0
    PID_View->Ilimit_flag = 0;//禁用积分分离
    PID_View->Irang = 40; //积分限幅
    PID_View->Output_Max = 50;

}

